﻿using System;
using System.Collections.Generic;
using Algorithms;

namespace AStarFinder
{    
    /*
     * 点结构
     */
    class CPoint
    {
        public CPoint()
        {

        }
        public CPoint(int x1, int y1)
        {
            x = x1;
            y = y1;
        }

        public int x = 0;
        public int y = 0;
    }

    /*
     * 路径
     */
    class CPath : List<CPoint>
    {

    }

    // 节点状态
    enum NODE_STATE
    {
        NS_NONE = 0,        // 新分配的节点
        NS_OPEN,            // 在打开队列
        NS_CLOSE,           // 在关闭队列
    }
    
    // 节点结构
    class CNode
    {
        public int m_nX = 0;   // 所在位置
        public int m_nY = 0;
        public int m_nGVal = 0;// 实际值
        public int m_nHVal = 0;// 启发值
        public CNode m_pPreNode = null;             // 父节点
        public NODE_STATE m_uCurState = NODE_STATE.NS_NONE;    // 节点所处状态
        public uint m_uSerial = 0;                  // 版本号

        public void Clear()
        {
            m_nX = 0;
            m_nY = 0;
            m_nGVal = 0;
            m_nHVal = 0;
            m_pPreNode = null;
            m_uCurState = NODE_STATE.NS_NONE;
            m_uSerial = 0;
        }
    }

    // 节点池
    class CNodePool
    {
        public void Reset(int nXsize, int nYSize)
        {
            // 新空间不可复用
            if (m_nXSize != nXsize || m_nYSize != nYSize)
            {
                m_lstNodes.Clear(); // 是否会自动回收？
                m_lstNodes = null;
                m_nXSize = nXsize;
                m_nYSize = nYSize;
            }

            // 重置新的空间,标记版本号
            if (m_lstNodes == null)
            {
                CNode[] nodes = new CNode[nXsize * nYSize];
                m_lstNodes.AddRange(nodes);
                m_uSerial = 1;

                for (int y = 0; y != nYSize; ++y)
                {
                    for (int x = 0; x != nXsize; ++x )
                    {
                        m_lstNodes[x + y * m_nXSize].m_nX = x;
                        m_lstNodes[x + y * m_nXSize].m_nY = y;
                    }
                }
            }
            else
            {
                // 复用已有空间
                ++m_uSerial;
            }
        }

        public CNode Get(int nX, int nY)
        {
            return m_lstNodes[nX + nY * m_nXSize];
        }

        // 有效点判断
        public bool IsGoodPoint(int nX, int nY)
        {
            if (nX < 0 || nY < 0 || nX >= m_nXSize || nY >= m_nYSize)
            {
                return false;
            }
            return true;
        }

        private List<CNode> m_lstNodes = new List<CNode>(); // 所有节点
        private int m_nXSize = 0;                           // 地图宽度
        private int m_nYSize = 0;                           // 地图高度
        private uint m_uSerial =0;                          // 序列号
    }

    /*
     *  open列表管理
     */
    class COpen
    {
        // 是否为空
        public bool IsEmpty()
        {
            return m_QueueNode.Count == 0;
        }

        // 清空数据
        public void Clear()
        {
            m_QueueNode.Clear(); 
        }

        // 取出最短路径节点
        public CNode GetFirst()
        {
            CNode node = m_QueueNode.Pop();
            node.m_uCurState = NODE_STATE.NS_CLOSE;
            return node;
        }

        // 放入一个节点
        public void Push(CNode node)
        {
            node.m_uCurState = NODE_STATE.NS_OPEN;
            m_QueueNode.Push(node);
        }

        // 修改一个节点
        public void Change(int x, int y)
        {
            for (int i = 0; i != m_QueueNode.Count; ++i)
            {
                if (m_QueueNode[i].m_nX == x && m_QueueNode[i].m_nY == y)
                {
                    m_QueueNode.Update(i);
                    return;
                }
            }
        }

        private class CNodeCompare : IComparer<CNode>
        {
            public int Compare(CNode x, CNode y)
            {
                return (x.m_nGVal+x.m_nHVal) - (y.m_nGVal + y.m_nHVal);
            }
        }
        private PriorityQueueB<CNode> m_QueueNode = new PriorityQueueB<CNode>(new CNodeCompare());
    }

    /*
     *  移动方向
     */
    enum NODE_DIR
    {
        ND_LEFT = 0,
        ND_LEFT_BOTTOM,
        ND_BOTTOM,
        ND_RIGHT_BOTTOM,
        ND_RIGHT,
        ND_RIGHT_TOP,
        ND_TOP,
        ND_LEFT_TOP,
        ND_MAX,
    }
    /*
     * 单位移动工具类
     */
    class CUnitMoveHelper
    {
        // 是否可到达
        public static bool CanMoveTo(ref CTitleMap map, int nSrcX, int nSrcY, int nDstX, int nDstY)
        {
            // 单步移动判断
            if (Math.Abs(nSrcX - nDstX) > 1 || Math.Abs(nSrcY - nDstY)>1)
            {
                return false;
            }

            // 障碍判断
            if (map.GetTitle(nSrcX, nSrcY) == (int)TITLE_TYPE.TT_OBSTACLE
                || map.GetTitle(nDstX, nDstY) == (int)TITLE_TYPE.TT_OBSTACLE)
            {
                return false;
            }

            // 倾斜移动障碍判断
            if (nSrcX != nDstX && nSrcY != nDstY)
            {
                if (map.GetTitle(nSrcX, nDstY) == (int)TITLE_TYPE.TT_OBSTACLE
                    || map.GetTitle(nDstX, nSrcY) == (int)TITLE_TYPE.TT_OBSTACLE)
                {
                    return false;
                }
            }
            return true;
        }

        // 目标方向下一坐标
        public static void Transform(NODE_DIR eNodeDir, ref int x, ref int y)
        {
            switch (eNodeDir)
            {
                case NODE_DIR.ND_LEFT:
                    --x;
                    break;
                case NODE_DIR.ND_LEFT_BOTTOM:
                    --x;
                    --y;
                    break;
                case NODE_DIR.ND_BOTTOM:
                    --y;
                    break;
                case NODE_DIR.ND_RIGHT_BOTTOM:
                    ++x;
                    --y;
                    break;
                case NODE_DIR.ND_RIGHT:
                    ++x;
                    break;
                case NODE_DIR.ND_RIGHT_TOP:
                    ++x;
                    ++y;
                    break;
                case NODE_DIR.ND_TOP:
                    ++y;
                    break;
                case NODE_DIR.ND_LEFT_TOP:
                    --x;
                    ++y;
                    break;
                case NODE_DIR.ND_MAX:
                    break;
                default:
                    break;
            }
        }

        // 启发函数
        public static int heuristics(int x1, int y1, int x2, int y2)
        {
            int nDx = Math.Abs(x1 - x2);
            int nDy = Math.Abs(y1 - y2);
            return (nDx + nDy) * 10;
        }

        // 移动距离
        public static int distance(NODE_DIR dir)
        {
            switch (dir)
            {
                case NODE_DIR.ND_LEFT:
                case NODE_DIR.ND_BOTTOM:
                case NODE_DIR.ND_RIGHT:
                case NODE_DIR.ND_TOP:
                    return 10;
                case NODE_DIR.ND_LEFT_BOTTOM:
                case NODE_DIR.ND_RIGHT_BOTTOM:
                case NODE_DIR.ND_RIGHT_TOP:
                case NODE_DIR.ND_LEFT_TOP:
                case NODE_DIR.ND_MAX:
                default:
                    return 14;
            }
        }
    }

    /*
     * A寻路 
     */
    class AStarFinder
    {
        // 寻路参数
        enum FIND_CFG
        {
            FC_MAX_STEP = 100000,   // 最大探索次数
        }

        // 取得路径
        public void find(ref CTitleMap map, int nSrcX, int nSrcY, int nDstX, int nDstY,
            ref CPath path, int nRangeX/*探索范围X*/, int nRangeY/*探索范围Y*/)
        {
            // 预清空
            path.Clear();
            m_noodPool.Reset(map.GetX(), map.GetY());
            m_open.Clear();

            // 设置起点
            CNode curNode = m_noodPool.Get(nSrcX, nSrcY);
            curNode.m_nGVal = 0;
            curNode.m_nHVal = CUnitMoveHelper.heuristics(nSrcX, nSrcY, nDstX, nDstY);
            m_open.Push(curNode);

            // 寻找路径
            uint uStep = 1; 
            while (false == m_open.IsEmpty())
            {
                // 最大探索次数
                if (++uStep > (int)FIND_CFG.FC_MAX_STEP)
                {
                    break;
                }

                // 取最优下一点,标记为已探索
                CNode bestNextNode = m_open.GetFirst();
                map.SetTitle(bestNextNode.m_nX, bestNextNode.m_nY, TITLE_TYPE.TT_CLOSE);

                // 合格的下一点
                if (curNode.m_nHVal > bestNextNode.m_nHVal)
                {
                    curNode = bestNextNode;
                }

                // 到达目标
                if (bestNextNode.m_nX == nDstX && bestNextNode.m_nY == nDstY)
                {
                    curNode = bestNextNode;
                    break;
                }

                // 探索下一点八方向
                for (NODE_DIR eDir = 0; eDir != NODE_DIR.ND_MAX; ++eDir)
                {
                    int nX = bestNextNode.m_nX;
                    int nY = bestNextNode.m_nY;
                    CUnitMoveHelper.Transform(eDir, ref nX, ref nY);

                    // 超出探索范围
                    if ((nRangeX > 0 && Math.Abs(nX - nSrcX) > nRangeX )
                        || (nRangeY > 0 && Math.Abs(nY - nSrcY) > nRangeY))
                    {
                        if (Math.Abs(nDstX - nSrcX) > nRangeX || Math.Abs(nDstY - nSrcY) > nRangeY)
                        {
                            curNode = bestNextNode;
                            m_open.Clear();
                            break;
                        }
                        else
                        {
                            // 尝试其它方西
                            continue;
                        }
                    }

                    // 阻挡
                    if (false == CUnitMoveHelper.CanMoveTo(ref map, bestNextNode.m_nX, bestNextNode.m_nY, nX, nY))
                    {
                        continue;
                    }

                    // 处理子节点
                    CNode testNode = m_noodPool.Get(nX, nY);
                    int uGVal = bestNextNode.m_nGVal + CUnitMoveHelper.distance(eDir);
                    switch(testNode.m_uCurState)
                    {
                        case NODE_STATE.NS_NONE:
                            // 未被访问过
                            testNode.m_nGVal = uGVal;
                            testNode.m_nHVal = CUnitMoveHelper.heuristics(nX, nY, nDstX, nDstY);
                            testNode.m_pPreNode = bestNextNode;
                            m_open.Push(testNode);
                            map.SetTitle(testNode.m_nX, testNode.m_nY, TITLE_TYPE.TT_OPEN);
                            break;

                        case NODE_STATE.NS_OPEN:
                            // 已经在OPEN列表,且从当前点更优
                            if (testNode.m_nGVal > uGVal)
                            {
                                testNode.m_nGVal = uGVal;
                                testNode.m_pPreNode = bestNextNode;
                                m_open.Change(testNode.m_nX, testNode.m_nY);
                            }
                            break;
                        case NODE_STATE.NS_CLOSE:
                            // 已访问过,且从当前点更优
                            if (testNode.m_nGVal > uGVal)
                            {
                                testNode.m_nGVal = uGVal;
                                testNode.m_pPreNode = bestNextNode;
                                m_open.Push(testNode);
                                map.SetTitle(testNode.m_nX, testNode.m_nY, TITLE_TYPE.TT_OPEN);
                            }
                            break;

                    }

                }
            } // end while


            // 生成路点
            while (curNode != null)
            {
                CPoint p = new CPoint(curNode.m_nX, curNode.m_nY);
                path.Add(p);
                curNode = curNode.m_pPreNode;
            }

            // 去除支线过程
            OptimalPath(ref path);
        }

        // 去除直线路点过程
        private void OptimalPath(ref CPath path)
        {
            if (path.Count < 3)
            {
                return;
            }

            CPath newPath = new CPath();
            newPath.Add(path[0]);
            int nDX = path[1].x - path[0].x;
            int nDY = path[1].y - path[0].y;
            int nNewDX = 0;
            int nNewDY = 0;
            for (int i = 2; i != path.Count; ++i)
            {
                nNewDX = path[i].x - path[i - 1].x;
                nNewDY = path[i].y = path[i - 1].y;
                if (nNewDX != nDX || nNewDY != nDY)
                {
                    newPath.Add(path[i - 1]);
                    nDX = nNewDX;
                    nDX = nNewDY;
                }
            }
            newPath.Add(path[path.Count - 1]);
            path = newPath;
        }



        private CNodePool   m_noodPool = new CNodePool();
        private COpen       m_open = new COpen();
    }
}